#ifndef slak_geom_Quat_h
#define slak_geom_Quat_h

namespace slak {
	namespace geom {

		typedef float Matrix[4][4]; 

		struct Quat {
			float x, y, z, w;

			Quat() : x(0), y(0), z(0), w(1) {}
			Quat(float _x, float _y, float _z, float _w)
				:x(_x), y(_y), z(_z), w(_w) {}
			Quat(const Quat& q) : x(q.x), y(q.y), z(q.z), w(q.w) {}

			void toMatrix(Matrix out) const 
			{
				float Nq = x*x + y*y + z*z + w*w;
				float s = (Nq > 0.f) ? (2.0f / Nq) : 0.f;
				float xs = x*s,  ys = y*s,   zs = z*s;
				float wx = w*xs, wy = w*ys,  wz = w*zs;
				float xx = x*xs, xy = x*ys,  xz = x*zs;
				float yy = y*ys, yz = y*zs,  zz = z*zs;

				out[0][0] = 1.0f - (yy + zz); out[1][0] = xy + wz;          out[2][0] = xz - wy;
				out[0][1] = xy - wz;          out[1][1] = 1.0f - (xx + zz); out[2][1] = yz + wx;
				out[0][2] = xz + wy;          out[1][2] = yz - wx;          out[2][2] = 1.0f - (xx + yy);
				out[0][3] = out[1][3] = out[2][3] = out[3][0] = out[3][1] = out[3][2] = 0.0f;
				out[3][3] = 1.0f;
			}

			Quat conjugate() const 
			{
				return Quat(-x,-y,-z,w);
			}

			Quat operator*(const Quat& q) const 
			{
				Quat res;
				res.w = w*q.w - x*q.x - y*q.y - z*q.z;
				res.x = w*q.x + x*q.w + y*q.z - z*q.y;
				res.y = w*q.y + y*q.w + z*q.x - x*q.z;
				res.z = w*q.z + z*q.w + x*q.y - y*q.x;
				return res;
			}

			Quat normalize() const 
			{
				float Nq = 1.0 / (x*x + y*y + z*z + w*w);
				return Quat(x*Nq, y*Nq, z*Nq, w*Nq);
			}
		};
	}
}

#endif
